/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.utx.spot.timesync.protocol;

import edu.utx.spot.timesync.ISyncClock;

/**
 *
 */
public class PIDClockController implements IClockController {

    // controller parameters
    private final int Kp; // proprotional
    private final int Ki; // integral
    private final int Kd; // derivative

    // controller state
    private long clockError = 0; // proportional
    private long totalError = 0; // integral
    private long deltaError = 0; // derivative

    private boolean clockErrorIsSet = false;

    // input values
    private long serverToClientOffset = 0;
    private long clientToServerOffset = 0;
    private long propagationDelay = 0;

    // save previous values for filtering
    private long prevServerToClientOffset = 0;
    private long prevClientToServerOffset = 0;
    private boolean filterInput;

    // reset clock if clock error is greater than threshold
    private final long CLOCK_RESET_THRESHOLD = 1000000; // 1 second

    private ISyncClock syncClock;

    public PIDClockController(ISyncClock syncClock, int Kp, int Ki, int Kd, boolean filter) {
        this.Kp = Kp;
        this.Ki = Ki;
        this.Kd = Kd;
        this.syncClock = syncClock;
        this.filterInput = filter;
    }

    // final int s2cDelay = (int)(delayTxTimestamp - delayRxTimestamp);
    // final int c2sDelay = (int)(delayRxTimestamp - delayTxTimestamp);
    // final int propagationDelay = (s2cDelay + c2sDelay) / 2;
    // final int timeError = s2cDelay - propagationDelay;

    public void updateServerToClientOffset(long offset) {
        if (filterInput) {
            serverToClientOffset = (offset/2 + prevServerToClientOffset/2);
            prevServerToClientOffset = offset;
        }
        else {
            serverToClientOffset = offset;
        }
        //updateController();
    }
    
    public void updateClientToServerOffset(long offset) {
        //clientToServerOffset = (int)offset;
        if (filterInput) {
            clientToServerOffset = (offset/2 + prevClientToServerOffset/2);
            prevClientToServerOffset = offset;
        }
        else {
            clientToServerOffset = offset;
        }
        updateController();
        adjustClock();
    }

    // debugging
    public void printState() {

    }

    void updateController() {
        final long oldClockError = clockError;
        propagationDelay = (serverToClientOffset/2 + clientToServerOffset/2);
        clockError = serverToClientOffset - propagationDelay;
        deltaError = clockErrorIsSet? clockError - oldClockError : 0;
        totalError += clockError;
        clockErrorIsSet = true;

        System.out.println("[pid] propagation delay "  + propagationDelay);
    }
    
    void adjustClock()
    {
        if (Math.abs(clockError) > CLOCK_RESET_THRESHOLD) {
            System.out.println("[pid] reset clock, offset = " + clockError);
            syncClock.setClockOffset(-clockError);

            // clear controller state
            clockError = 0;
            deltaError = 0;
            totalError = 0;
            clockErrorIsSet = false;
            return;
        }

        //
        // PID controller:
        //
        long output = 0;

        System.out.println("[pid] controller input:");
        System.out.println("[pid]    error " + clockError);
        System.out.println("[pid]    total " + totalError);
        System.out.println("[pid]    delta " + deltaError);
        
        output += (Kp == 0) ? 0 : clockError/Kp;
        output += (Ki == 0) ? 0 : totalError/Ki;
        output += (Kd == 0) ? 0 : deltaError/Kd;

        System.out.println("[pid] controller output: " + output);
        syncClock.setClockRate(output);
    }
}
